package com.zyzg.utils;

import com.alibaba.fastjson2.JSONObject;
import com.zyzg.domain.GPSPoint;

import java.util.ArrayList;
import java.util.List;

public class GPSFilter {
    private static final double EARTH_RADIUS = 6371000; // 地球半径（米）
    private static final double COORDINATE_THRESHOLD = 0.1; // 坐标相同阈值（米）

    // 计算两点间距离（Haversine公式）
    public static double calculateDistance(GPSPoint p1, GPSPoint p2) {
        double lat1 = Math.toRadians(p1.getLat());
        double lon1 = Math.toRadians(p1.getLon());
        double lat2 = Math.toRadians(p2.getLat());
        double lon2 = Math.toRadians(p2.getLon());

        double dLat = lat2 - lat1;
        double dLon = lon2 - lon1;

        double a = Math.sin(dLat / 2) * Math.sin(dLat / 2) +
                Math.cos(lat1) * Math.cos(lat2) *
                        Math.sin(dLon / 2) * Math.sin(dLon / 2);
        double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
        return EARTH_RADIUS * c;
    }

    public static double calculateDistance2(double lng01, double lat01, double lng02, double lat02) {
        double lat1 = Math.toRadians(lat01);
        double lon1 = Math.toRadians(lng01);
        double lat2 = Math.toRadians(lng02);
        double lon2 = Math.toRadians(lat02);

        double dLat = lat2 - lat1;
        double dLon = lon2 - lon1;

        double a = Math.sin(dLat / 2) * Math.sin(dLat / 2) +
                Math.cos(lat1) * Math.cos(lat2) *
                        Math.sin(dLon / 2) * Math.sin(dLon / 2);
        double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
        return EARTH_RADIUS * c;
    }

    // 过滤速度过高的点及坐标相同的点
    public static List<JSONObject> filterBySpeed(List<JSONObject> points, double maxSpeed) {
        List<JSONObject> filtered = new ArrayList<>();
        if (points.isEmpty()) return filtered;
        filtered.add(points.get(0)); // 保留第一个点

        for (int i = 1; i < points.size(); i++) {
            JSONObject curr = points.get(i);
            JSONObject lastFiltered = filtered.get(filtered.size() - 1);

            GPSPoint prevPoint = new GPSPoint(
                    lastFiltered.getDouble("lat"),
                    lastFiltered.getDouble("lng"),
                    DateUtils.getTimestampByFormat(lastFiltered.getString("gpstime"), "yyyyMMddHHmmss")
            );
            GPSPoint currPoint = new GPSPoint(
                    curr.getDouble("lat"),
                    curr.getDouble("lng"),
                    DateUtils.getTimestampByFormat(curr.getString("gpstime"), "yyyyMMddHHmmss")
            );

            double distance = calculateDistance(prevPoint, currPoint);
            long timeDiff = (currPoint.getTimestamp() - prevPoint.getTimestamp()) / 1000; // 转为秒

            // 过滤坐标相同点（距离小于阈值）
            if (distance < COORDINATE_THRESHOLD) {
                continue;
            }

            if (timeDiff == 0) {
                continue; // 避免除零错误
            }

            double speed = distance / timeDiff;
            if (speed <= maxSpeed) {
                filtered.add(curr);
            }
        }
        return filtered;
    }
}